package source.calcCmd;


public class IdealMove {

	public static State idealMove(State start, long nanoTime){
		// radius in meters
		double r;
		// angular velocity
		double omega;
		// A vector pointing to the center of rotation
		Vec toO;
		// Rotation center
		Vec o;
		// Rotation in radians
		double alpha;
		// t in SEC
		double ts = nanoTime / RealLifeProperties.s2ns;
		// the function makes modofications on this "virtual" state
		State ret = start;
		
		// If the trajectory is not linear
		if (Math.abs(ret.vR-ret.vL)>0.001){
			r = RealLifeProperties.l*(ret.vL+ret.vR)/(ret.vR-ret.vL);
			
			omega = (ret.vR-ret.vL)/(2*RealLifeProperties.l);
			// toO = [-sin(fi);cos(fi)]
			toO = new Vec(-1*(Math.sin(ret.fi)),Math.cos(ret.fi));
			// o = ret +  r*toO
			o = new Vec(ret.pos.addNR(toO.multiply(r)));
			
			alpha = omega * ts;
			
			Vec tmp = ret.pos.add(o.multiplyNR(-1));
			// new position
			ret.pos = o.add(new Vec(tmp.getX()*Math.cos(alpha)-tmp.getY()*Math.sin(alpha),
							tmp.getX()*Math.sin(alpha)+tmp.getY()*Math.cos(alpha)));
			// new orientation
			ret.fi += alpha;
		} else {
			double v = ret.vR;
			Vec i = new Vec((Math.cos(ret.fi)),Math.sin(ret.fi));
			ret.pos.add(i.multiplyNR(v*ts));
		}
		
		ret.fi = RealLifeProperties.normalizeOrient(ret.fi);
		
		return ret;
	}
	
	
}
